﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Common
{
    public class Cls_LvBo
    {
        float passBandXH, passBandXL, passBandYH, passBandYL;//通带上下限频率
        float[] numeratorCoeffX = new float[50];
        float[] dominatorCoeffX = new float[50];
        float[] numeratorCoeffY = new float[50];
        float[] dominatorCoeffY = new float[50];
        float[] px0 = new float[1024];
        float[] px1 = new float[1024];
        float[] px2 = new float[1024];
        float[] px3 = new float[1024];
        int ORDERS = 4;//滤波器阶数
        int SECTIONS = 4;//滤波器节数
        int DB = 60;//阻带衰减值
        float[] x = new float[1024];
        float[] y = new float[1024];
        float[] new_X = new float[1024];
        float[] new_Y = new float[1024];
        public double[] lvbo_x=new double[1024];
        public double[] lvbo_y = new double[1024];
        public void LvBo(int doubleFrequency, float[] x_value,float[] y_value,string type)
        {
            lvbo_x = new double[1024];
            lvbo_y = new double[1024];
            int sampleMul = doubleFrequency;
            x = x_value;
            y = y_value;

            if (type != "通频" && type != "")
            {
                switch (type)
                {

                    case "基频":
                        passBandXH = Convert.ToSingle(1.1 * 1.0 / sampleMul);
                        passBandYH = Convert.ToSingle(1.1 * 1.0 / sampleMul);
                        passBandXL = Convert.ToSingle(0.9 * 1.0 / sampleMul);
                        passBandYL = Convert.ToSingle(0.9 * 1.0 / sampleMul);
                        break;
                    case "2倍频":
                        passBandXH = Convert.ToSingle(2.15 * 1.0 / sampleMul);
                        passBandYH = Convert.ToSingle(2.15 * 1.0 / sampleMul);
                        passBandXL = Convert.ToSingle(1.85 * 1.0 / sampleMul);
                        passBandYL = Convert.ToSingle(1.85 * 1.0 / sampleMul);
                        break;
                    case "0.5倍频":
                        passBandXH = Convert.ToSingle(0.55 * 1.0 / sampleMul);
                        passBandYH = Convert.ToSingle(0.55 * 1.0 / sampleMul);
                        passBandXL = Convert.ToSingle(0.45 * 1.0 / sampleMul);
                        passBandYL = Convert.ToSingle(0.45 * 1.0 / sampleMul);
                        break;
  
                    default:
                        break;
                }

                iirbcf(3, 3, SECTIONS, ORDERS, 0, passBandXL, passBandXH, 0, DB, numeratorCoeffX, dominatorCoeffX);//生成滤波器系数
                iirbcf(3, 3, SECTIONS, ORDERS, 0, passBandYL, passBandYH, 0, DB, numeratorCoeffY, dominatorCoeffY);
                filterc(numeratorCoeffX, dominatorCoeffX, ORDERS, SECTIONS, x, new_X, x.Length, px0, px1);
                filterc(numeratorCoeffY, dominatorCoeffY, ORDERS, SECTIONS, y, new_Y, y.Length, px2, px3);

                for (int n = 0; n < 512; n++)
                {
                    new_X[n] = new_X[n + 512];
                    new_Y[n] = new_Y[n + 512];
                }
            }
            else
            {

                for (int n = 0; n < 1024; n++)
                {
                    new_X[n] = x[n];
                    new_Y[n] = y[n];
                }

            }
            double x_min = 0.0;
            double y_min = 0.0;
            double x_max = 0.0;
            double y_max = 0.0;
            for (int i = 0; i < 1024; i++)
            {
                lvbo_x[i] = Math.Round(new_X[i], 3);
                lvbo_y[i] = Math.Round(new_Y[i], 3);

                if (x_min > Math.Round(new_X[i], 3))
                {
                    x_min = Math.Round(new_X[i], 3);
                }
                if (y_min > Math.Round(new_Y[i], 3))
                {
                    y_min = Math.Round(new_Y[i], 3);
                }


                if (x_max < Math.Round(new_X[i], 3))
                {
                    x_max = Math.Round(new_X[i], 3);
                }
                if (y_max < Math.Round(new_Y[i], 3))
                {
                    y_max = Math.Round(new_Y[i], 3);
                }


            }

            //chart1.ChartAreas[0].AxisX.Minimum = x_min - 1;
            //chart1.ChartAreas[0].AxisX.Maximum = x_max + 1;


            //chart1.ChartAreas[0].AxisY.Minimum = y_min - 1;
            //chart1.ChartAreas[0].AxisY.Maximum = y_max + 1;
        }




        public void iirbcf(int ifilt, int band, int ns, int n, float f1, float f2, float f3, float f4, float db, float[] b, float[] a)
        {
            int k;
            float omega = 0;
            float lamda = 0;
            float epslon = 0;
            float fl = 0;
            float fh = 0; ;
            float[] d = new float[5];
            float[] c = new float[5];
            if ((band == 1) || (band == 4)) fl = f1;//如果是低通和带阻，则把f1作为下限截止频率
            if ((band == 2) || (band == 3)) fl = f2;//如果是高通和带通，则把f2作为下限截止频率
            if (band <= 3) fh = f3;//如果不是带阻，则把f3作为上限截止频率
            if (band == 4) fh = f4;//如果是带阻，则把f4作为上限截止频
            if (ifilt < 3)
            {
                switch (band)
                {
                    case 1:
                        lamda = Convert.ToSingle(Math.Pow(10.0, (db / 20.0)));
                        epslon = Convert.ToSingle(lamda / ncoshl(Convert.ToDouble(2.0 * ns * ncoshl(omega))));
                        break;
                    case 2:
                        {
                            omega = warp(f2) / warp(f1);
                            break;
                        }
                    case 3:
                        {
                            omega = omin(bpsub(warp(f1), fh, fl), bpsub(warp(f4), fh, fl));
                            break;
                        }
                    case 4:
                        {
                            omega = omin(Convert.ToSingle(1.0 / bpsub(warp(f2), fh, fl)), Convert.ToSingle(1.0 / bpsub(warp(f3), fh, fl)));
                            lamda = Convert.ToSingle(Math.Pow(10.0, (db / 20.0)));
                            epslon = Convert.ToSingle(lamda / ncoshl(Convert.ToDouble(2.0 * ns * ncoshl(omega))));
                            break;
                        }
                }
            }
            for (k = 0; k < ns; k++)
            {
                switch (ifilt)//滤波器类型选择
                {
                    case 1:
                        {
                            chebyi(2 * ns, k, 4, epslon, d, c);//切比雪夫i型
                            break;
                        }
                    case 2:
                        {
                            chebyii(2 * ns, k, 4, omega, lamda, d, c);//切比雪夫ii型
                            break;
                        }
                    case 3:
                        {
                            bwtf(2 * ns, k, 4, d, c);
                            break;
                        }

                }
                float[] new_a = a.Skip(k * (n + 1) + 0).Take(a.Length).ToArray();
                float[] new_b = b.Skip(k * (n + 1) + 0).Take(b.Length).ToArray();
                fblt(d, c, n, band, fl, fh, new_b, new_a);//
                int acount = 0;
                int bcount = 0;
                for (int i = k * (n + 1) + 0; i < a.Length; i++)
                {
                    a[i] = new_a[acount];
                    acount++;
                }
                for (int i = k * (n + 1) + 0; i < b.Length; i++)
                {
                    b[i] = new_b[bcount];
                    bcount++;
                }
            }
        }

        public void filterc(float[] b, float[] a, int n, int ns, float[] x, float[] y, int len, float[] px, float[] py)
        {
            int i, j, k, n1;
            n1 = n + 1;
            for (j = 0; j < ns; j++)
            {
                for (k = 0; k < len; k++)
                {
                    px[j * n1 + 0] = x[k];
                    y[k] = x[k];
                    y[k] = b[j * n1 + 0] * px[j * n1 + 0];
                    for (i = 1; i <= n; i++)
                    {
                        y[k] += b[j * n1 + i] * px[j * n1 + i] - a[j * n1 + i] * py[j * n1 + i];
                    }
                    /* if(fabs(x[k]>1.0e10))
      {
            printf("This is an unstable filter!\n");
            //exit(0);
      }*/
                    for (i = n; i >= 2; i--)
                    {
                        px[j * n1 + i] = px[j * n1 + i - 1];
                        py[j * n1 + i] = py[j * n1 + i - 1];
                    }
                    px[j * n1 + 1] = px[j * n1 + 0];
                    py[j * n1 + 1] = y[k];
                }
            }
        }

        public float warp(float f) // z=tan(pi*f)
        {

            float pi, z;
            pi = Convert.ToSingle(4.0 * Math.Atan(1.0));
            z = Convert.ToSingle(Math.Tan(pi * f));
            return (z);
        }

        public float omin(float om1, float om2)//比较绝对值大小
        {
            float z, z1, z2;
            z1 = Math.Abs(om1);
            z2 = Math.Abs(om2);
            z = (z1 < z2) ? z1 : z2;
            return (z);
        }

        public float bpsub(float om, float fh, float fl)
        {
            float z;
            z = (om * om - warp(fh) * warp(fl)) / ((warp(fh) - warp(fl)) * om);
            return (z);
        }

        public double ncoshl(double x)
        {
            double z;
            z = Math.Log(x + Math.Sqrt(x * x - 1.0));
            return (z);
        }

        public void chebyi(int ln, int k, int n, float ep, float[] d, float[] c)
        {
            int i;
            float pi, gam, omega, sigma;
            pi = Convert.ToSingle(4.0 * Math.Atan(1.0));
            gam = Convert.ToSingle(Math.Pow(((1.0 + Math.Sqrt(1.0 + ep * ep)) / ep), 1.0 / ln));
            sigma = Convert.ToSingle(0.5 * (1.0 / gam - gam) * Math.Sin((2 * (k + 1) - 1) * pi / (2 * ln)));
            omega = Convert.ToSingle(0.5 * (1.0 / gam + gam) * Math.Cos((2 * (k + 1) - 1) * pi / (2 * ln)));
            for (i = 0; i <= n; i++)
            {
                d[i] = 0;
                c[i] = 0;
            }
            if (((ln % 2) == 1) && ((k + 1) == (ln + 1) / 2))
            {
                d[0] = -sigma;
                c[0] = d[0];
                c[1] = 1;
            }
            else
            {
                c[0] = sigma * sigma + omega * omega;
                c[1] = -2 * sigma;
                c[2] = 1;
                d[0] = c[0];
                if (((ln % 2) == 0) && (k == 0))
                {
                    d[0] = Convert.ToSingle(d[0] / Math.Sqrt(1.0 + ep * ep));
                }
            }
        }

        public void chebyii(int ln, int k, int n, float ws, float att, float[] d, float[] c)
        {
            int i;
            float pi, gam, alpha, beta, sigma, omega, scln, scld;
            pi = Convert.ToSingle(4.0 * Math.Atan(1.0));
            gam = Convert.ToSingle(Math.Pow((att + Math.Sqrt(att * att - 1.0)), 1.0 / ln));
            alpha = Convert.ToSingle(0.5 * (1.0 / gam - gam) * Math.Sin((2.0 * (k + 1) - 1) * pi / (2.0 * ln)));
            beta = Convert.ToSingle(0.5 * (1.0 / gam + gam) * Math.Cos((2.0 * (k + 1) - 1) * pi / (2.0 * ln)));
            sigma = ws * alpha / (alpha * alpha + beta * beta);
            omega = Convert.ToSingle(-1.0 * ws * beta / (alpha * alpha + beta * beta));
            for (i = 0; i <= n; i++)
            {
                d[i] = 0;
                c[i] = 0;
            }
            if (((ln % 2) == 1) && ((k + 1) == (ln + 1) / 2))
            {
                d[0] = -1 * sigma;
                c[0] = d[0];
                c[1] = 1;
            }
            else
            {
                scln = sigma * sigma + omega * omega;
                scld = Convert.ToSingle(Math.Pow((ws / Math.Cos((2.0 * (k + 1) - 1) * pi / (2.0 * ln))), 2));
                d[0] = scln * scld;
                d[2] = scln;
                c[0] = d[0];
                c[1] = -2 * sigma * scld;
                c[2] = scld;
            }

        }

        public void bwtf(int ln, int k, int n, float[] d, float[] c)//巴特沃斯滤波器
        {
            int i;
            float pi, tmp;
            pi = Convert.ToSingle(4.0 * Math.Atan(1.0));
            d[0] = 1.0f;
            c[0] = 1.0f;
            for (i = 1; i <= n; i++)
            {
                d[i] = 0;
                c[i] = 0;
            }
            tmp = Convert.ToSingle((k + 1.0) - (ln + 1.0) / 2.0);
            //tmp = (k + 1) - (ln + 1) / 2;
            if (tmp == 0.0)
            {
                c[1] = 1.0f;
            }
            else
            {
                c[1] = Convert.ToSingle(-2.0 * Math.Cos((2 * (k + 1) + ln - 1) * pi / (2 * ln)));
                c[2] = 1.0f;
            }

        }


        public void fblt(float[] d, float[] c, int n, int band, float fln, float fhn, float[] b, float[] a)
        {
            int i, k, m, n1, n2, ls;
            float pi = 0;
            float w = 0;
            float w0 = 0;
            float w1 = 0;
            float w2 = 0;
            float tmp = 0;
            float tmpd = 0;
            float tmpc = 0;
            float[] work = null;
            //bilinear(d, c, b, a, n);
            pi = Convert.ToSingle(4 * Math.Atan(1));
            w1 = Convert.ToSingle(Math.Tan(pi * fln));
            for (i = n; i >= 0; i--)
            {
                if ((c[i] != 0.0) || (d[i] != 0.0))
                    break;
            }
            m = i;
            switch (band)
            {
                case 1:
                case 2:
                    {
                        n2 = m;
                        n1 = n2 + 1;
                        if (band == 2)
                        {
                            for (i = 0; i <= m / 2; i++)
                            {
                                tmp = d[i];
                                d[i] = d[m - i];
                                d[m - i] = tmp;
                                tmp = c[i];
                                c[i] = c[m - i];
                                c[m - i] = tmp;
                            }
                        }
                        for (i = 0; i <= m; i++)
                        {
                            d[i] = Convert.ToSingle(d[i] / Math.Pow(w1, i));
                            c[i] = Convert.ToSingle(c[i] / Math.Pow(w1, i));
                        }
                        break;
                    }
                case 3:
                case 4:
                    {
                        n2 = 2 * m;
                        n1 = n2 + 1;
                        work = new float[n1 * n1];
                        w2 = Convert.ToSingle(Math.Tan(pi * fhn));
                        w = w2 - w1;
                        w0 = w1 * w2;
                        if (band == 4)
                        {
                            for (i = 0; i <= m / 2; i++)
                            {
                                tmp = d[i];
                                d[i] = d[m - i];
                                d[m - i] = tmp;
                                tmp = c[i];
                                c[i] = c[m - i];
                                c[m - i] = tmp;
                            }
                        }
                        for (i = 0; i <= n2; i++)
                        {
                            work[0 * n1 + i] = 0;
                            work[1 * n1 + i] = 0;
                        }
                        for (i = 0; i <= m; i++)
                        {
                            tmpd = Convert.ToSingle(d[i] * Math.Pow(w, (m - i)));
                            tmpc = Convert.ToSingle(c[i] * Math.Pow(w, (m - i)));
                            for (k = 0; k <= i; k++)
                            {
                                ls = m + i - 2 * k;
                                tmp = combin(i, i) / (combin(k, k) * combin(i - k, i - k));
                                work[0 * n1 + ls] += Convert.ToSingle(tmpd * Math.Pow(w0, k) * tmp);
                                work[1 * n1 + ls] += Convert.ToSingle(tmpc * Math.Pow(w0, k) * tmp);
                            }
                        }
                        for (i = 0; i <= n2; i++)
                        {
                            d[i] = work[0 * n1 + i];//???
                            c[i] = work[1 * n1 + i];
                        }

                        break;

                    }
            }
            bilinear(d, c, b, a, n);
        }

        public float combin(int i1, int i2)
        {
            int i;
            float s;
            s = 1;
            if (i2 == 0) return (s);
            for (i = i1; i > (i1 - i2); i--)
            {
                s *= i;
            }
            return (s);
        }

        public void bilinear(float[] d, float[] c, float[] b, float[] a, int n)//双线性Z变换，d,c为模拟滤波器传递函数的分子分母多项式的系数向量，                        //b,a为数字滤波器传递函数的分子分母多项式的系数向量
        {
            int i, j, n1;
            float sum = 0;
            float atmp = 0;
            float scale = 0;
            float[] temp = null;
            n1 = n + 1;
            temp = new float[n1 * n1];
            for (j = 0; j <= n; j++)
            {
                temp[j * n1 + 0] = 1;
            }
            sum = 1;
            for (i = 1; i <= n; i++)
            {
                sum = sum * (float)(n - i + 1) / (float)i;
                temp[0 * n1 + i] = sum;//?????????
            }
            for (i = 1; i <= n; i++)
                for (j = 1; j <= n; j++)
                {
                    temp[j * n1 + i] = temp[(j - 1) * n1 + i] - temp[j * n1 + i - 1] - temp[(j - 1) * n1 + i - 1];
                }
            for (i = n; i >= 0; i--)
            {
                b[i] = 0;
                atmp = 0;
                for (j = 0; j <= n; j++)
                {
                    b[i] = b[i] + temp[j * n1 + i] * d[j];
                    atmp = atmp + temp[j * n1 + i] * c[j];
                }
                scale = atmp;
                if (i != 0) a[i] = atmp;
            }
            for (i = 0; i <= n; i++)
            {
                b[i] = b[i] / scale;
                a[i] = a[i] / scale;
            }
            a[0] = 1;



        }
    }
}
